/**
 * This software is distributed under the terms of the MIT License.
 * Copyright (c) 2020 CrobTech.
 * Author: Jason Zhu
 * Contributors: https://gitee.com/JasonZhu2021/tmf8801_tof_demo
 */

/**************************************************************************************
 * INCLUDE
 **************************************************************************************/

#include "TMF8801_Generic.h"

#define __delay  HAL_Delay
/**************************************************************************************
 * PRIVATE FUNC
 **************************************************************************************/
static int32_t ReadRegWrap(void *Handle, uint8_t Reg, uint8_t *pData, uint16_t Length);
static int32_t WriteRegWrap(void *Handle, uint8_t Reg, uint8_t *pData, uint16_t Length);
/**
 * @brief                   IO 读取函数
 * @param  Handle           控制对象
 * @param  Reg              寄存器地址
 * @param  pData            数据地址
 * @param  Length           数据长度
 * @return int32_t          IO 操作状态
 */
static int32_t ReadRegWrap(void *Handle, uint8_t Reg, uint8_t *pData, uint16_t Length)
{
    TMF8801_Object_t *pObj = (TMF8801_Object_t *)Handle;
    return pObj->IO.ReadReg(pObj->IO.Address, Reg, pData, Length);
}
/**
 * @brief                   IO 读取函数
 * @param  Handle           控制对象
 * @param  Reg              寄存器地址
 * @param  pData            数据地址
 * @param  Length           数据长度
 * @return int32_t          IO 操作状态
 */
static int32_t WriteRegWrap(void *Handle, uint8_t Reg, uint8_t *pData, uint16_t Length)
{
    TMF8801_Object_t *pObj = (TMF8801_Object_t *)Handle;
    return pObj->IO.WriteReg(pObj->IO.Address, Reg, pData, Length);
}
/**************************************************************************************
 * PUBLIC MEMBER FUNCTIONS
 **************************************************************************************/
/**
 * @brief                   注册绑定总线IO硬件接口
 * @param  pObj             TMF8801 对象
 * @param  pIO              TMF8801 IO对象
 * @return int32_t          注册绑定结果
 */
int32_t TMF8801_RegisterBusIO(TMF8801_Object_t *pObj, TMF8801_IO_t *pIO)
{
  int32_t ret = TMF8801_OK;
  if(pObj == NULL)
  {
    ret = TMF8801_ERROR;
  }
  else
  {
    pObj->IO.Init       = pIO->Init;
    pObj->IO.Address    = pIO->Address;
    pObj->IO.WriteReg   = pIO->WriteReg;
    pObj->IO.ReadReg    = pIO->ReadReg;

    pObj->Ctx.read_reg  = ReadRegWrap;
    pObj->Ctx.write_reg  = WriteRegWrap;
    pObj->Ctx.handle    = pObj;

    if(pObj->IO.Init == NULL)
    {
      ret = TMF8801_ERROR;
    }
    else if(pObj->IO.Init() != TMF8801_OK)
    {
      ret = TMF8801_ERROR;
    }
  }
  return ret;
}
/**
 * @brief                   TMF8801初始化
 * @param  pObj             TMF8801 对象
 * @param  _calib_data      校准参数
 * @param  _algo_state      运行参数
 * @return int32_t          初始化结果
 */
int32_t TMF8801_Init(TMF8801_Object_t *pObj, uint8_t *_calib_data, uint8_t *_algo_state)
{
  uint8_t id;
  pObj->Err = None;
  tmf8801_device_reset(&(pObj->Ctx));
  if(!TMF8801_WaitForCpuReady(pObj))
    return FALSE;

  TMF8801_ReadID(pObj, &id);
  if(id != TMF8801_CHIP_ID)
  {
    pObj->Err = ChipId;
    return FALSE;
  }

  TMF8801_LoadApplication(pObj);
  if(!TMF8801_WaitForApplication(pObj))
    return FALSE;

  tmf8801_write_reg(&(pObj->Ctx), TMF8801_COMMAND, DOWNLOAD_CALIB_AND_STATE);
  tmf8801_load_calibdata(&(pObj->Ctx), _calib_data, 14U);
  tmf8801_load_algostate(&(pObj->Ctx), _algo_state, 11U);
  tmf8801_write_reg(&(pObj->Ctx), CMD_DATA7, 0x03);
  tmf8801_write_reg(&(pObj->Ctx), CMD_DATA6, 0x23);
  tmf8801_write_reg(&(pObj->Ctx), CMD_DATA5, 0x00);
  tmf8801_write_reg(&(pObj->Ctx), CMD_DATA4, 0x00);
  tmf8801_write_reg(&(pObj->Ctx), CMD_DATA3, 0x00);
  tmf8801_write_reg(&(pObj->Ctx), CMD_DATA2, 0x64);
  tmf8801_write_reg(&(pObj->Ctx), CMD_DATA1, 0xff);
  tmf8801_write_reg(&(pObj->Ctx), CMD_DATA0, 0xff);
  tmf8801_write_reg(&(pObj->Ctx), TMF8801_COMMAND, DISTANCE_MEASURE_MODE_1);
	
	return TRUE;
}
/**
 * @brief                   TMF8801 出厂校准
 * @param  pObj             TMF8801 对象
 * @param  pCalibData       校准后的参数
 * @return int32_t          校准结果
 */
int32_t TMF8801_FactoryCalib(TMF8801_Object_t *pObj, uint8_t *pCalibData)
{
  uint8_t id;

  TMF8801_ReadID(pObj, &id);
  if(id != TMF8801_CHIP_ID)
  {
    pObj->Err = ChipId;
    return FALSE;
  } 

  tmf8801_device_reset(&(pObj->Ctx));
  if(!TMF8801_WaitForCpuReady(pObj))
    return FALSE;

  TMF8801_LoadApplication(pObj);
  if(!TMF8801_WaitForApplication(pObj))
    return FALSE;

  tmf8801_write_reg(&(pObj->Ctx), TMF8801_COMMAND, FACTORY_CALIB);
  if(!TMF8801_WaitForFactortCalib(pObj))
    return FALSE;

  pObj->IO.ReadReg(pObj->IO.Address, FACTORY_CALIB_0, pCalibData, 14U);

  return TRUE;
}
/**
 * @brief                   等待CPU准备好
 * @param  pObj             TMF8801 对象
 * @return int32_t          等待结果
 */
int32_t TMF8801_WaitForCpuReady(TMF8801_Object_t *pObj)
{
  static uint8_t cpu_ready_timeout_ms = 100;
  static uint8_t cpu_ready_timeout_increment_ms = 10;

  uint8_t t = 0;
  for(;t < cpu_ready_timeout_ms; t += cpu_ready_timeout_increment_ms)
  {
    __delay(cpu_ready_timeout_increment_ms);
    if(tmf8801_is_cpu_ready(&(pObj->Ctx)))
    {
      return TRUE;
    }
  }
  pObj->Err = Timeout; 
  return FALSE;
}
/**
 * @brief                   获取测量距离
 * @param  pObj             TMF8801 对象
 * @return uint16_t         测量结果
 */
uint16_t TMF8801_GetDistance_mm(TMF8801_Object_t *pObj)
{
  ObjectDetectionData data;
  tmf8801_read_detection_result(&(pObj->Ctx), (uint8_t *)data.buf);
  return data.field.distance_peak_0_mm;
}
/**
 * @brief                   获取芯片ID
 * @param  pObj             TMF8801 对象
 * @param  Id               获取的ID值
 * @return int32_t          获取结果
 */
int32_t TMF8801_ReadID(TMF8801_Object_t *pObj, uint8_t *Id)
{
    if(tmf8801_device_id_get(&(pObj->Ctx),Id) != TMF8801_OK)
    {
        return TMF8801_ERROR;
    }
    return TMF8801_OK;
}
/**
 * @brief                   加载应用程序
 * @param  pObj             TMF8801 对象
 * @return int32_t          加载程序结果
 */
int32_t TMF8801_LoadApplication(TMF8801_Object_t *pObj)
{
  return tmf8801_write_reg(&(pObj->Ctx), TMF8801_APPREQID, TMF8801_APP);
}

int32_t TMF8801_WaitForApplication(TMF8801_Object_t *pObj)
{
  static uint8_t app_loaded_timeout_ms = 100;
  static uint8_t app_loaded_timeout_increment_ms = 10;

  uint8_t t = 0;
  for(;t < app_loaded_timeout_ms; t += app_loaded_timeout_increment_ms)
  {
    __delay(app_loaded_timeout_ms);
    if(tmf8801_is_app_ready(&(pObj->Ctx)) == APP_SUCCESS)
    {
      return TRUE;
    }
  }
  pObj->Err = Timeout; 
  return FALSE; 
}
/**
 * @brief                   等待校准完成
 * @param  pObj             TMF8801 对象
 * @return int32_t          校准结果
 */
int32_t TMF8801_WaitForFactortCalib(TMF8801_Object_t *pObj)
{
  static uint16_t factory_calib_timeout_ms = 1000;
  static uint16_t factory_calib_timeout_increment_ms = 100;

  uint8_t reg_val;
  uint8_t t = 0;
  for(;t < factory_calib_timeout_ms; t += factory_calib_timeout_increment_ms)
  {
    __delay(factory_calib_timeout_increment_ms);
    tmf8801_read_reg(&(pObj->Ctx), REGISTER_CONTENTS, &reg_val);
    if(reg_val == CALIB_DATA)
    {
      return TRUE;
    }
  }
  pObj->Err = Timeout; 
  return FALSE; 
}



